#ifndef SERVODRIVE_H
#define SERVODRIVE_H

#include "EthercatSlave.h"
#include <QDebug>
#include <QThread>

#define CIA_STATE_WORD          0x6041
#define CIA_CONTROL_WORD        0x6040
#define CIA_OP_MODE_DISPLAY     0x6061
#define CIA_OP_MODE             0x6060
#define CIA_POS_FB              0x6064
#define CIA_POS_CMD             0x607A
#define CIA_VEL_FB              0x606C
#define CIA_VEL_CMD             0x60FF
#define CIA_VEL_OFFSET_CMD      0x60B1
#define CIA_ERROR_CODE          0x603F


namespace ECatDrv
{

//#pragma pack(push, 1)  // 避免填充字节
//typedef struct {
//    int32_t position_actual_value; // 0x6064
//    int32_t velocity_actual_value; // 0x606C
//} TxPDO_xServo;
//#pragma pack(pop)

#pragma pack(push, 1)  // 禁止字节对齐（防止结构体填充）
typedef struct {
    int32_t  target_position;    // 0x607A:00
    int32_t  target_velocity;    // 0x60FF:00
    int32_t  velocity_offset;    // 0x60B1:00
    int16_t  target_torque;      // 0x6071:00
    int16_t  torque_offset;      // 0x60B2:00
    uint16_t controlword;        // 0x6040:00
    uint8_t  modes_of_operation; // 0x6060:00 (USINT)
    uint8_t  rxdummy;            // 0x2001:00 (USINT)
} xServoPDOOutput;
#pragma pack(pop)

#pragma pack(push, 1)  // 禁止字节对齐（防止结构体填充）
typedef struct {
    int32_t  position_actual_value;    // 0x6064:00 DINT
    int32_t  velocity_actual_value;    // 0x606C:00 DINT
    int32_t  follow_error;            // 0x60F4:00 DINT
    int16_t  torque_actual_value;     // 0x6077:00 INT
    uint16_t statusword;              // 0x6041:00 UINT
    int16_t  analog_output_1;         // 0x2401:00 INT
    int16_t  analog_output_2;         // 0x2402:00 INT
    uint8_t  modes_of_operation_display; // 0x6061:00 USINT
    int8_t   txdummy;                 // 0x2000:00 SINT
    uint16_t error_code;              // 0x603F:00 UINT
} xServoPDOInput;
#pragma pack(pop)


// eep_man
// 添加自定义伺服标识
enum ServoDeviceID
{
    ServoDevicedID_InoSV660N = 0x00100000,
    ServoDevicedID_xServo = 0x00078557,
};

// eep_id
// 同上
enum ServoProductCode
{
    ServoProductCode_InoSV660N = 0x000C010D,
    ServoProductCode_xServo_1 = 0x00016101,
    ServoProductCode_xServo_2 = 0x00016200,
    ServoProductCode_xServo_3 = 0x00016100,
    ServoProductCode_xDrive = 0x00015370,
};

// 伺服OP模式
enum ServoOpMode
{
    OP_CSP       =   8,
    OP_CSV       =   9,
    OP_CST       =  10,
    OP_OTHER     =   0,
};

//// 伺服控制模式
//enum ServoControlMode2
//{
//    STATE_NOT_READY =               0x00,
//    STATE_SWITCH_ON_DISABLED =      0x40,
//    STATE_READY_TO_SWITCH_ON =      0x21,
//    STATE_SWITCHED_ON =             0x31,
//    STATE_OPERATION_ENABLED =       0x33,
//    STATE_FAULT =                   0x08
//};

enum ServoControlMode
{
    NotReadyToSwicthOn,
    SwitchOnDisabled,
    ReadyToSwitchOn,
    SwitchOn,
    OperationEnable,
    QuickStopActive,
    Fault
};


struct ServoState
{
    masterState EcatState;
    ServoOpMode op;
    ServoControlMode ctrlMode;
    bool powerEnable;   // 功率上电标志

    int32_t  velocity;
    uint16_t errorCode;
    uint16_t stateVal;
    uint8_t opVal;
    uint8_t version[4]; // 伺服版本
};

struct ServoControl
{
    bool ServoEnable;
    ServoOpMode op;

    uint32_t positionCmd;
    int32_t velocityCmd;
};


class ServoDrive : public EthercatSlave
{
public:
    ServoDrive(unsigned int eep_man, unsigned int eep_id, const std::string& slave_name, int slave_addr,
               slaveType slave_type);

    ServoDrive() = default;


    /**
     * @brief 读取SDO数据
     * @param  addr             SDO消息地址
     * @param  size             SDO数据大小
     * @param  buf              SDO数据
     */
    void readSDO(uint16_t addr, int *size, uint8_t *buf);

    /**
     * @brief 清除错误
     */
    void ClearFault(void);

    /**
     * @brief 伺服状态更新
     */
    virtual void StMachineUpdate(void);

    /**
     * @brief 获取程序版本号
     * @return std::string 版本号
     */
    std::string GetVersionStr(void);

    void GetVersion(void);


    /**
     * @brief 判断是否为Bootloader状态
     */
    bool isBootloaderState(void);

    /**
     * @brief 获取CiA402标准的状态和op模式
     * @param  state            状态
     * @param  op               op模式
     */
    void GetSlaveState(uint16_t &state, uint8_t &op);

    /**
     * @brief 获取错误状态
     * @param  faultcode        错误码
     * @param  error            错误描述
     */
    virtual void GetFaultInfo(uint16_t &faultcode, std::string &error);

    /**
     * @brief 获取从站sdo数据
     * @param  addr
     * @param  size   sdo数据(单个数)
     * @return int
     */
    int GetSlaveSdo(int addr, int size, unsigned int* recv);

    /**
     * @brief 设置从站sdo数据
     * @param  addr
     * @param  size   sdo数据(单个数)
     * @return int
     */
    int SetSlaveSdo(uint16 addr, int size, unsigned int* data);

    /**
     * @brief 伺服上使能
     * @return true 成功
     * @return false 失败
     */
    virtual bool ServoEnable(void);

    /**
     * @brief 伺服下使能
     */
    virtual void ServoDisable(void);

    virtual void JogOn(int vel);

    virtual void JogOff();

    virtual void initPdoOutput();

    virtual void initPdoInput();

    ServoState m_state;
    ServoControl m_control;
};

}


#endif // SERVODRIVE_H
